from src.utils import singleton
from typing import Dict, Tuple, List
import queue
from src.grpc.clients.robot_data.robot_data_client import RobotDataClient
from src.grpc.clients.image_harmony.image_harmony_client import ImageHarmonyClient
from src.grpc.clients.target_tracking.target_tracking_client import TargetTrackingClient
from src.config.config import Config
import yaml
import threading
import time
import logging

logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')

class TaskInfo:
    def __init__(self, taskId: int):
        self.id: int = taskId

        self.__image_harmony_address: List[str, str] = []
        self.__image_harmony_client: ImageHarmonyClient = None
        self.__loader_args_hash: int = 0  # image harmony中加载器的hash值

        self.__target_tracking_address: List[str, int] = []
        self.__target_tracking_client: TargetTrackingClient = None

        self.__robot_address: List[str, int] = ['', -1]
        self.__robot_data_client: RobotDataClient = None
        self.image_id_queue: queue.Queue[int] = queue.Queue()
        self.__stop_event = threading.Event()
        self.__track_thread = None  # 用于跟踪线程的引用
    
    def set_pre_service(self, pre_service_name: str, pre_service_ip: str, pre_service_port: int, args: Dict[str, str]):
        if 'image harmony gRPC' == pre_service_name:
            self.__image_harmony_address = [pre_service_ip, pre_service_port]
            self.__image_harmony_client = ImageHarmonyClient(pre_service_ip, pre_service_port)
            if 'LoaderArgsHash' not in args:
                raise ValueError('Argument "LoaderArgsHash" is required but not set.')
            self.__loader_args_hash = int(args['LoaderArgsHash'])
        if 'target tracking' == pre_service_name:
            self.__target_tracking_address = [pre_service_ip, pre_service_port]
            self.__target_tracking_client = TargetTrackingClient(pre_service_ip, pre_service_port, self.id)
            self.__target_tracking_client.filter.add('person')
    
    def set_cur_service(self, args: Dict[str, str]):
        if 'RobotIp' in args:
            self.__robot_address[0] = args['RobotIp']
        if 'RobotPort' in args:
            self.__robot_address[1] = int(args['RobotPort'])
    
    def check(self) -> None:
        if not self.__image_harmony_client:
            raise ValueError('Error: image_harmony_client not set.')
        if not self.__target_tracking_client:
            raise ValueError('Error: target_tracking_client not set.')
        if not self.__robot_address:
            raise ValueError('Error: robot address not set.')

    def start(self) -> None:
        self.check()
        self.__image_harmony_client.connect_image_loader(self.__loader_args_hash)
        self.__robot_data_client = RobotDataClient(self.__robot_address[0], self.__robot_address[1])
        self.__stop_event.clear()  # 确保开始时事件是清除状态
        self.__track_thread = threading.Thread(target=self.auto_track)
        self.__track_thread.start()

    def auto_track(self):
        while not self.__stop_event.is_set():
            try:
                image_id, width, height = self.__image_harmony_client.get_image_size_by_image_id(0)
                bboxes = self.__target_tracking_client.get_result_by_image_id(image_id, True)
                sorted_keys = sorted(bboxes.keys())
                # TODO 目前追踪id最小的目标
                bbox = bboxes[sorted_keys[0]][0]
                area = (bbox.x2-bbox.x1)*(bbox.y2-bbox.y1)
                if area < 0.2:
                    self.__robot_data_client.set_twist_keyboard('i')
                if area > 0.5:
                    self.__robot_data_client.set_twist_keyboard(',')
                # center_x = (bbox.x2+bbox.x1)/2
                # center_y = (bbox.y2+bbox.y1)/2
                # if center_x < 0.4:
                #     # 人偏左，右转
                #     self.__robot_data_client.set_twist_keyboard('l')
                # if center_x > 0.6:
                #     # 人偏右，左转
                #     self.__robot_data_client.set_twist_keyboard('j')
            except Exception as e:
                logging.error(e)


    def stop(self):
        self.__stop_event.set()  # 设置事件，通知线程停止
        if self.__track_thread:
            self.__track_thread.join()  # 等待线程结束
                                                                                                                                         
@singleton
class TaskManager:
    def __init__(self):
        self.tasks: Dict[int, TaskInfo] = {}
        self.__lock = threading.Lock()
    
    def stop_task(self, task_id: int):
        with self.__lock:
            if task_id in self.tasks:
                self.tasks[task_id].stop()
                del self.tasks[task_id]